#include <fstream>
#include "SDTerrain.h"
#include "SDUtils.h"

using namespace std;

// load a terrain from a file
SDTerrain::SDTerrain(char* filename)
{
  ifstream fin(filename);
  double scale;

  // read the trimesh directly
  fin >> mNumVertices >> mNumFaces >> scale;
  
  mVertices = new double [mNumVertices * 3];
  mFaces = new int [mNumFaces * 3];
  mNormals = 0;
  
  for (int i = 0; i < mNumVertices * 3; i++) {
    fin >> mVertices[i];
    mVertices[i] *= scale;
  }
  
  for (int i = 0; i < mNumFaces * 3; i++) {
    fin >> mFaces[i];
  }
 
  calculateNormals();
}

// build the trimesh in ODE
void SDTerrain::buildInSpace(dSpaceID space)
{
  mTrimeshData = dGeomTriMeshDataCreate();

  dGeomTriMeshDataBuildDouble1(mTrimeshData, mVertices, 3 * sizeof(double),
			       mNumVertices, mFaces, mNumFaces * 3,
			       3 * sizeof(int), mNormals);
  
  mTrimeshGeom = dCreateTriMesh(space, mTrimeshData, 0, 0, 0);
  dGeomSetData(mTrimeshGeom, (void*)this);
}

// position the terrain object
void SDTerrain::setPosition(const dVector3 pos)
{
  dGeomSetPosition(mTrimeshGeom, pos[0], pos[1], pos[2]);
}

// orient the terrain object
void SDTerrain::setOrientation(const dQuaternion orient)
{
  dGeomSetQuaternion(mTrimeshGeom, orient);
}

// draw the trimesh with drawstuff routines
void SDTerrain::dsDraw()
{
  const dReal *pos = dGeomGetPosition(mTrimeshGeom);
  const dReal *rot = dGeomGetRotation(mTrimeshGeom);

  for (int i = 0; i < mNumFaces; i++) {
    dsSetColor(0.7, 0.4, 0.2);
    dsDrawTriangleD(pos, rot, &mVertices[mFaces[i * 3 + 0] * 3],
		    &mVertices[mFaces[i * 3 + 1] * 3],
		    &mVertices[mFaces[i * 3 + 2] * 3], 1);
  }
}


// calculate normals for all the faces
void SDTerrain::calculateNormals()
{
  if (mNormals != 0) {
    delete mNormals;
  }

  mNormals = new double [mNumFaces * 3];

  for (int i = 0; i < mNumFaces; i++) {
    double *v1, *v2, *v3;
    double v1v2[3], v1v3[3];
    double len;
    
    v1 = &mVertices[mFaces[i*3+0]*3];
    v2 = &mVertices[mFaces[i*3+1]*3];
    v3 = &mVertices[mFaces[i*3+2]*3];

    v1v2[0] = v2[0] - v1[0];
    v1v2[1] = v2[1] - v1[1];
    v1v2[2] = v2[2] - v1[2];
    v1v3[0] = v3[0] - v1[0];
    v1v3[1] = v3[1] - v1[1];
    v1v3[2] = v3[2] - v1[2];

    mNormals[i*3+0] = v1v2[1]*v1v3[2] - v1v2[2]*v1v3[1];
    mNormals[i*3+1] = v1v2[2]*v1v3[0] - v1v2[0]*v1v3[2];
    mNormals[i*3+2] = v1v2[0]*v1v3[1] - v1v2[1]*v1v3[0];

    len = sqrt(pow(mNormals[i*3+0], 2) + pow(mNormals[i*3+1], 2) +
	       pow(mNormals[i*3+2], 2));
    
    mNormals[i*3+0] /= len;
    mNormals[i*3+1] /= len;
    mNormals[i*3+2] /= len;
  }
}
